/**
 ******************************************************************************
 * Copyright (c) 2022 - ~, SCUT-RobotLab Development Team
 * @file    Control_Node.cpp
 * @author  LJY
 * @brief   Code for .
 * @date   2022-02-28
 * @version 1.0
 * @par Change Log:
 * <table>
 * <tr><th>Date        <th>Version  <th>Author     <th>Description
 * <tr><td>2022-02-28  <td> 1.0     <td>LJY  			<td>Creator
 * </table>
 *
 ******************************************************************************
 * @attention
 *
 * if you had modified this file, please make sure your code does not have many
 * bugs, update the version Number, write dowm your name and the date, the most
 * important is make sure the users will have clear and definite understanding
 * through your new brief.
 *
 * <h2><center>&copy; Copyright (c) 2022 - ~,SCUT-RobotLab Development Team.
 * All rights reserved.</center></h2>
 ******************************************************************************
 */
#include "Control_Node.h"
#include "common/Math/orientation_tools.h"
#include "std_msgs/Int8.h"

void Route_Planning::run(geometry_msgs::Point& loc) {
    auto& seResult = _robotRunner->_stateEstimator->getResult();

    // 获取当前位置
    Vec4<float> Pw;
    Vec3<float> position(loc.x, loc.y, 0);
    Pw.block(0, 0, 3, 1) = position;
    Pw(3) = 1;
    Vec4<float> Pb = T * Pw;
    float current_x = Pb(0);
    float current_y = Pb(1);
    float current_yaw = _yaw;
    current_state << current_x, current_y, current_yaw;
    // printf("x: %f, y: %f\n", current_x, current_y);

    switch (process_flag) {
        case 0:
            _robotRunner->updateRCgait(0);

            // 首先矫正航向角
            if (fabs(target.pro_yaw - current_yaw) > 0.1) {
                float target_yaw = limit(((target.pro_yaw - current_yaw) > 0 ? 1. : -1.) * 0.5, -0.3, 0.3);
                _robotRunner->updateRCvel(0, 0, target_yaw);
                // printf("pro_yaw: %f, %f\n", target.pro_yaw, current_yaw);
            }

            // 逼近目标位置
            else if (fabs(target.x - current_x) > MAX_ERR_X || fabs(target.y - current_y) > MAX_ERR_Y) {
                float vel_x = limit(0.5 * (target.x - current_x), -0.25, 0.25);
                float vel_y = limit(0.5 * (target.y - current_y), -0.15, 0.15);
                float omega = limit(0.4 * (target.pro_yaw - current_yaw), -0.1, 0.1);
                _robotRunner->updateRCvel(vel_x, vel_y, omega);
                // printf("vel: %f, %f, %f\n", vel_x, vel_y, omega);
                // printf("position: %f, %f, %f, %f\n", target.x, current_x, target.y, current_y);

            } else {
                process_flag = 1;
            }
            break;
        case 1:
            _robotRunner->updateRCgait(0);
            // 逼近目标姿态
            if (fabs(target.yaw - current_yaw) > MAX_ERR_YAW) {
                float omega = limit(0.5 * (target.yaw - current_yaw), -0.3, 0.3);
                _robotRunner->updateRCvel(0, 0, omega);
                // printf("yaw: %f, %f\n", target.yaw, current_yaw);
            } else {
                process_flag = 2;
            }
            break;
        case 2:
            if (routes_cmd.size() > 0) {
                // 更新目标点
                target = routes_cmd.front();
                target_state << target.x, target.y, target.yaw;
                routes_cmd.pop();

                // 更新状态
                _robotRunner->updateRCgait(0);
                calculateTransitionMatrix(seResult.rBody, position);
                process_flag = 0;
            } else if (routes_data.size() > 0) {
                addTarget(routes_data.front().data, routes_data.front().type);
                routes_data.pop();
            } else {
                _robotRunner->updateRCgait(4);
            }
        default:
            break;
    }
    updateRouteState(current_state, target_state);
    // printf("current: %f, target: %f\n", current_state(2), target_state(2));
}

void Route_Planning::addTarget(float* data, uint8_t type) {
    auto& seResult = _robotRunner->_stateEstimator->getResult();
    Route_Cmd cmd;
    Route_Data r_data;
    // 直线目标点
    if (!type) {
        cmd.x = data[0];
        cmd.y = data[1];
        cmd.yaw = _yaw + data[2] * Ang2Deg;
        cmd.pro_yaw = _yaw;
        routes_cmd.push(cmd);
    }

    // 曲线目标点
    else {
        // 圆形
        // int block = data[1] / 30.;
        // for (int i = 0; i < block; i++) {
        //     cmd.x = 0.58 * data[0];
        //     cmd.y = 0;
        //     cmd.pro_yaw = 1.57 - i * 0.79;
        //     cmd.yaw = cmd.pro_yaw ;
        //     routes.push(cmd);
        // }

        // // 最后修改姿态
        // cmd.x = 0;
        // cmd.y = 0;
        // cmd.yaw = data[2] * 0.0087;
        // cmd.pro_yaw = seResult.rpy(2);
        // routes.push(cmd);

        // 方形
        // for(int i = 0; i < 5; i ++){
        //     if(i == 0 || i == 4){
        //         cmd.x = data[0] / 2;
        //     }
        //     else {
        //         cmd.x = data[0];
        //     }
        //     cmd.y = 0;
        //     cmd.pro_yaw = _yaw + M_PI_2 - i * M_PI_2;
        //     cmd.yaw = cmd.pro_yaw ;
        //     routes_cmd.push(cmd);
        // }

        // cmd.x = 0;
        // cmd.y = 0;
        // cmd.yaw = _yaw + data[2] * Ang2Deg - 2 * M_PI;
        // routes_cmd.push(cmd);
        for (int i = 0; i < 6; i++) {
            r_data.type = 0;
            r_data.data[1] = 0;
            switch (i) {
                case 0:
                    r_data.data[0] = 0;
                    r_data.data[2] = 90;
                    break;
                case 1:
                    r_data.data[0] = data[0] / 2;
                    r_data.data[2] = -90;
                    break;
                case 5:
                    r_data.data[0] = data[0] / 2;
                    r_data.data[2] = data[2] - 90;
                    break;
                default:
                    r_data.data[0] = data[0];
                    r_data.data[2] = -90;
                    break;
            }
            routes_data.push(r_data);
        }
    }
}

void Route_Planning::calculateTransitionMatrix(Mat3<float> R, Vec3<float> P) {
    Mat4<float> T_temp = Mat4<float>::Zero();
    T_temp.block(0, 0, 3, 3) = R.transpose();
    T_temp(0, 3) = P(0);
    T_temp(1, 3) = P(1);
    T_temp(2, 3) = P(2);
    T_temp(3, 3) = 1;
    T = T_temp.inverse();
    // printf("%f, %f, %f, %f\n", T(0,0), T(0,1), T(0,2), T(0,3));
    // printf("%f, %f, %f, %f\n", T(1,0), T(1,1), T(1,2), T(1,3));
    // printf("%f, %f, %f, %f\n", T(2,0), T(2,1), T(2,2), T(2,3));
}

float Route_Planning::limit(float in, float min, float max) {
    if (in < min)
        return min;
    else if (in > max)
        return max;
    else
        return in;
}

void Control_Node::Draw_debugData(Debug_GraphType graph_type, Debug_NumType graph_num, float value, std::string name) {
    DebugData.position[graph_type * 3 + graph_num] = value;
    DebugData.name[graph_type * 3 + graph_num] = name;
}

void Control_Node::FreqTimer_Callback(const ros::TimerEvent& event) {
    // sensor_msgs::JointState state = *planner->getRouteState();
    // if(Ctrl_Mode == 2) State_Puber.publish(state);
}

void Control_Node::DebugTimer_Callback(const ros::TimerEvent& event) {
    // debugData_Puber.publish(DebugData);
}

void Control_Node::Node_Init(void) {
    // create puber and suber
    RobotCmd_Puber = n->advertise<common::RobotCommand>("/quadruped/control/command", 10);
    debugData_Puber = n->advertise<sensor_msgs::JointState>("/quadruped/qt_monitor/graph_data", 10);
    AUcmd_Puber = n->advertise<std_msgs::Int8>("/quadruped/control/au_cmd", 10);
    State_Puber = n->advertise<sensor_msgs::JointState>("/quadruped/control/state_data", 10);
    LegData_Suber = n->subscribe("/quadruped/control/leg_data", 100, &Control_Node::LegData_Callback, this);
    RCCmd_Suber = n->subscribe("/quadruped/control/rc_cmd", 100, &Control_Node::RCCmd_Callback, this);
    TouchForce_Suber = n->subscribe("/quadruped/control/touch_force", 100, &Control_Node::TouchForce_Callback, this);
    CheaterState_Suber =
        n->subscribe("/quadruped/control/cheater_state", 100, &Control_Node::CheaterState_Callback, this);
    ImuData_Suber = n->subscribe("/quadruped/control/imu_data", 100, &Control_Node::ImuData_Callback, this);
    Nav_Suber = n->subscribe("/quadruped/control/nav_data", 100, &Control_Node::Navigation_Callback, this);
    Route_Suber = n->subscribe("/quadruped/control/route_data", 100, &Control_Node::RoutePlan_Callback, this);
    AUCmd_Suber = n->subscribe("/quadruped/control/remote_au_cmd", 100, &Control_Node::AUCmd_Callback, this);
    Loc_Suber = n->subscribe("/quadruped/control/dog_pos", 100, &Control_Node::Location_Callback, this);
    // Init TImer
    freq_timer = n->createTimer(ros::Duration(0.01), &Control_Node::FreqTimer_Callback, this, false, false);
    debug_timer = n->createTimer(ros::Duration(0.005), &Control_Node::DebugTimer_Callback, this, false, false);

    // create runner
    _robotRunner = new RobotRunner(new SRPQ_Controller());
    _robotRunner->init();

    // planner
    planner = new Route_Planning(_robotRunner, n);
}

void Control_Node::Run(void) {
    ros::Rate rate(1.f / _robotRunner->dt());
    int count = 0;

    // test
    rc_control_settings rc_cmd;
    rc_cmd.mode = RC_mode::LOCOMOTION;
    //  rc_cmd.mode = RC_mode::STAND_UP;
    rc_cmd.variable[0] = 4;    // gait 0->trot,4->stand
    rc_cmd.variable[1] = 0;    // jump_trrigle
    rc_cmd.variable[2] = 0;    // jump_type
    rc_cmd.v_des[0] = 0.0;
    rc_cmd.v_des[1] = 0.0;
    rc_cmd.omega_des[2] = 0.0;
    rc_cmd.height_variation = -0.29;
    _robotRunner->updateRC(&rc_cmd);
    RCCmd_Available = true;

    // init debug data
    for (int i = 0; i < 12; i++) {
        DebugData.position.push_back(0);
        DebugData.name.push_back("data");
    }

    // start timer
    freq_timer.start();
    debug_timer.start();

    ros::AsyncSpinner s(4);
    s.start();
    while (ros::ok()) {
        if (legdataUpdate && !_robotRunner->controlParameters.cheater_mode) {
            _robotRunner->_stateEstimator->run();
            legdataUpdate = false;
            Estimator_Available = true;
            auto& rs = _robotRunner->_stateEstimator->getResult();
            planner->updateYaw(rs.rpy(2));
            // printf("pitch:%f\n", rs.rpy(1));
            // printf("yaw:%f\n", planner->getYaw());
        } else if (_robotRunner->controlParameters.cheater_mode) {
            Estimator_Available = true;
        }
    }
    ros::shutdown();
}

void Control_Node::updateRobotCommand(const RobotCommand* cmd) {
    float* debug = _robotRunner->getLegdebugData();
    common::RobotCommand _cmd;
    std_msgs::Int8 au_cmd;
    au_cmd.data = _au_cmd;
    // printf("%d\n",_au_cmd);
    if (cmd != nullptr) {
        if (iter > 1000) {
            convertToMsg(cmd, &_cmd);
            RobotCmd_Puber.publish(_cmd);
            AUcmd_Puber.publish(au_cmd);
        }
    }
    iter++;
}

void Control_Node::LegData_Callback(const common::LegDataConstPtr& msg) {
    static int init_flag = 0;
    LegData _data;
    convertFromMsg(msg, &_data);
    _robotRunner->updateData(&_data);

    if (init_flag > 20) {
        if (!LegData_Available)
            LegData_Available = true;
        legdataUpdate = true;
    } else {
        init_flag++;
    }

    if (RCCmd_Available && LegData_Available && Estimator_Available && (CheaterState_Available || ImuData_Available)) {
        updateRobotCommand(_robotRunner->run());
    }
    freq_count++;
}

void Control_Node::RCCmd_Callback(const common::RCCommandConstPtr& msg) {
    static int init_flag = 0;
    rc_control_settings _rc;
    _rc.mode = msg->mode;
    _rc.height_variation = msg->height_variation;
    for (int i = 0; i < 12; i++) {
        if (i < 2) {
            _rc.p_des[i] = msg->p_des[i];
            _rc.test_type[i] = msg->test_type[i];
        }
        if (i < 3) {
            _rc.omega_des[i] = msg->omega_des[i];
            _rc.v_des[i] = -msg->v_des[i];
            _rc.rpy_des[i] = -msg->rpy_des[i];
            _rc.variable[i] = msg->variable[i];
        }
        _rc.test_data[i] = msg->test_data[i];
    }

    // update au
    _au_cmd = msg->test_type[1];

    // update nav
    Ctrl_Mode = msg->test_type[0];
    switch (Ctrl_Mode) {
        case 0:
            _robotRunner->updateRC(&_rc);
            break;

        case 1:
            break;

        case 2:
            planner->run(robot_loc);
            break;

        case 3:
            break;
        default:
            break;
    }
    // printf("%d\n", Ctrl_Mode);

    if (!RCCmd_Available && init_flag > 20) {
        RCCmd_Available = true;
    } else
        init_flag++;
}

void Control_Node::CheaterState_Callback(const common::CheaterStateConstPtr& msg) {
    static int init_flag = 0;
    static int64_t iter = 0;
    CheaterState<float> state;
    state.orientation.w() = msg->bodyOrientation.w;
    state.orientation.x() = msg->bodyOrientation.x;
    state.orientation.y() = msg->bodyOrientation.y;
    state.orientation.z() = msg->bodyOrientation.z;

    state.acceleration << msg->acceleration[0], msg->acceleration[1], msg->acceleration[2];
    state.omegaBody << msg->omegaBody[0], msg->omegaBody[2], msg->omegaBody[1];
    state.position << msg->position[0], msg->position[1], msg->position[2];
    state.vBody << msg->vBody[0], msg->vBody[1], msg->vBody[2];
    _robotRunner->updataCheaterState(&state);
    if (!CheaterState_Available && init_flag > 20) {
        CheaterState_Available = true;
    } else
        init_flag++;
}

void Control_Node::ImuData_Callback(const common::ImuDataConstPtr& msg) {
    static int init_flag = 0;
    VectorNavData nav;
    nav.quat.w() = msg->quat.w;
    nav.quat.x() = msg->quat.x;
    nav.quat.y() = msg->quat.y;
    nav.quat.z() = msg->quat.z;

    nav.gyro << msg->gyro[0], msg->gyro[1], msg->gyro[2];
    nav.accelerometer << msg->accel[0], msg->accel[1], msg->accel[2];
    _robotRunner->updateVectorNavData(&nav);
    if (!ImuData_Available && init_flag > 20) {
        ImuData_Available = true;
    } else
        init_flag++;
}

void Control_Node::TouchForce_Callback(const common::TouchForceConstPtr& msg) {}

void Control_Node::Navigation_Callback(const geometry_msgs::TwistConstPtr& msg) {
    std_msgs::Int8 cmd;
    if (Ctrl_Mode == 1) {
        _robotRunner->updateRCvel(msg->linear.x, msg->linear.y, msg->angular.z);
        _robotRunner->updateRCgait(0);
    }
}

void Control_Node::RoutePlan_Callback(const sensor_msgs::JointState& msg) {
    int route_num = msg.position.size() / 4;
    Route_Data data;
    for (int i = 0; i < route_num; i++) {
        data.type = msg.position[i * 4];
        if (!data.type) {
            for (int j = 1; j < 4; j++) {
                data.data[j - 1] = msg.position[i * 4 + j];
            }
            planner->addRouteData(data);
        }

        else {
            for (int k = 0; k < 6; k++) {
                data.type = 0;
                data.data[1] = 0;
                switch (k) {
                    case 0:
                        data.data[0] = 0;
                        data.data[2] = 90;
                        break;
                    case 1:
                        data.data[0] = msg.position[i * 4 + 1] / 2;
                        data.data[2] = -90;
                        break;
                    case 5:
                        data.data[0] = msg.position[i * 4 + 1] / 2;
                        data.data[2] = msg.position[i * 4 + 3] - 90;
                        break;
                    default:
                        data.data[0] = msg.position[i * 4 + 1];
                        data.data[2] = -90;
                        break;
                }
                planner->addRouteData(data);
            }
        }
    }
}

void Control_Node::AUCmd_Callback(const std_msgs::Int8ConstPtr& msg) {
    static float vel = 0.15;
    if (msg->data == -2) {
        AU_Cmd_Enable = true;
        printf("enable!\n");
    }

    else if (msg->data == -3) {
        _robotRunner->updateRCgait(4);
        AU_Cmd_Enable = false;
        printf("disable!\n");
    }

    if (AU_Cmd_Enable) {
        switch (msg->data) {
            // 停止运动
            case 0x03:
                _robotRunner->updateRCgait(4);
                printf("stop!\n");
                break;
            // 前进
            case 0x04:
                _robotRunner->updateRCgait(0);
                vel = 0.15;
                _robotRunner->updateRCvel(vel, 0, 0);
                printf("move forward!\n");
                break;

            // 后退
            case 0x05:
                _robotRunner->updateRCgait(0);
                vel = -0.15;
                _robotRunner->updateRCvel(vel, 0, 0);
                printf("move back!\n");
                break;

            // 加速
            case 0x06:
                if (vel > 0) {
                    vel += 0.05;
                    vel = fmin(vel, 0.3);
                } else {
                    vel -= 0.05;
                    vel = fmax(vel, -0.3);
                }
                _robotRunner->updateRCvel(vel, 0, 0);
                printf("move quckly!\n");
                break;

            // 减速
            case 0x07:
                if (vel > 0) {
                    vel -= 0.05;
                    vel = fmax(vel, 0.05);
                } else {
                    vel += 0.05;
                    vel = fmin(vel, -0.05);
                }
                _robotRunner->updateRCvel(vel, 0, 0);
                printf("move slowly!\n");
                break;

            // 左转
            case 0x08:
                _robotRunner->updateRCgait(0);
                _robotRunner->updateRCvel(0, 0, 0.2);
                printf("turn left!\n");
                break;

                // 右转
            case 0x09:
                _robotRunner->updateRCgait(0);
                _robotRunner->updateRCvel(0, 0, -0.2);
                printf("turn right!\n");
                break;
            default:
                break;
        }
    }
}

void Control_Node::Location_Callback(const geometry_msgs::PointConstPtr& msg) {
    robot_loc.x = msg->x;
    robot_loc.y = msg->y;
    robot_loc.z = msg->z;
    // printf("%f, %f, %f\n", robot_loc.x, robot_loc.y, robot_loc.z);
}

int main(int argc, char** argv) {
    // node init
    ros::init(argc, argv, "Control_Node", ros::init_options::AnonymousName);
    ros::NodeHandle n;
    ros::Duration(3).sleep();

    Control_Node* ctrl_node;
    if (argc > 1) {
        if (argv[1][0] == 't') {
            ctrl_node = new Control_Node(&n, true);
        } else if (argv[1][0] == 'f') {
            ctrl_node = new Control_Node(&n, false);
        }
    } else {
        ctrl_node = new Control_Node(&n, true);
    }
    if (ctrl_node != nullptr)
        ctrl_node->Run();

    return 0;
}

/************************ COPYRIGHT(C) SCUT-RobotLab Development Team **************************/
